from Methods.block import *
import numpy as np
#输入3个点，输出6个面，用面的4点坐标表示
def point_3_to_surface_6(args):  # 输入3个点坐标，输出六个面，4*6个坐标
    args = point_3_to_9(args)
    surface1_point4 = [args[0], args[1], args[2], args[3]] # 面1-6的顺序分别是前后上下左右，每个面的顺序是左上角开始，顺时针走
    surface2_point4 = [args[4], args[5], args[6], args[7]]
    surface3_point4 = [args[4], args[5], args[1], args[0]]
    surface4_point4 = [args[7], args[6], args[2], args[3]]
    surface5_point4 = [args[4], args[0], args[3], args[7]]
    surface6_point4 = [args[5], args[1], args[2], args[6]]
    # 输出1个大数组,每个数组包括6个元组（代表6个面），每个元组包括4个坐标（代表面的4个点）
    # return [surface1_point4, surface2_point4, surface3_point4, surface4_point4, surface5_point4 ,surface6_point4]
    block_tuple6 = [surface1_point4, surface2_point4, surface3_point4, surface4_point4, surface5_point4,
                    surface6_point4]
    return block_tuple6

#输入3个点，输出8个点
def point_3_to_8(args):                #坐标3-8
    min_x = args[0][0]
    max_x = args[2][0]
    min_y = args[0][1]
    max_y = args[2][1]
    min_z = args[0][2]
    max_z = args[2][2]

    a1 = [min_x,max_y,max_z]
    a2 = [max_x,max_y,max_z]                   #a前点,b后点,得到的数据类型是list
    a3 = [max_x,max_y,min_z]
    a4 = [min_x,max_y,min_z]
    b1 = [min_x,min_y,max_z]
    b2 = [max_x,min_y,max_z]
    b3 = [max_x,min_y,min_z]
    b4 = [min_x,min_y,min_z]

    args = [a1, a2, a3, a4, b1, b2, b3, b4]
    # print(args)
    return args
#输入8个点，输出3个点
def point_8_to_3(args):
    a = args[7]
    b = args[1]
    c = [(a[0]+b[0])/2,(a[1]+b[1])/2,(a[2]+b[2])/2]
    args = [a,c,b]

    return args

#输入管道信息，输出四条与流向平行的棱
def pipe_to_pipes(pipe):
    import numpy as np
    a = dian2(pipe[0],pipe[1],pipe[2],pipe[3])
    a1 = [a[0],a[1]]
    a2 = [a[3],a[2]]
    a3 = [a[4], a[5]]
    a4 = [a[7], a[6]]
    a5 = [pipe[0],pipe[1]]
    a6 = [a1,a2,a3,a4,a5]
    # print(a6)
    return a6

#输入起点终点宽高，输出8个点
def dian2(p1,p2,width,height):#第二种即两个中心点z不一样
    u=0
    p3=np.zeros(3)
    p4=np.zeros(3)
    p5=np.zeros(3)
    p6=np.zeros(3)
    p_1=np.zeros(3)
    p_2=np.zeros(3)
    p_3=np.zeros(3)
    p_4=np.zeros(3)
    p_5=np.zeros(3)
    p_6=np.zeros(3)
    p_7=np.zeros(3)
    p_8=np.zeros(3)
    if p1[1]-p2[1]==0:
        u=1
    if u==0:
        k=-(p1[0]-p2[0])/(p1[1]-p2[1])
        x=width/(2*(1+k**2)**0.5)
        p3[0]=p1[0]+x
        p3[1]=p1[1]+k*x
        p3[2]=p1[2]
        p4[0]=p1[0]-x
        p4[1]=p1[1]-k*x
        p4[2]=p1[2]
        p5[0]=p2[0]+x
        p5[1]=p2[1]+k*x
        p5[2]=p2[2]
        p6[0]=p2[0]-x
        p6[1]=p2[1]-k*x
        p6[2]=p2[2]
    else :
        p3[0]=p1[0]
        p3[1]=p1[1]+width/2
        p3[2]=p1[2]
        p4[0]=p1[0]
        p4[1]=p1[1]-width/2
        p4[2]=p1[2]
        p5[0]=p2[0]
        p5[1]=p2[1]+width/2
        p5[2]=p2[2]
        p6[0]=p2[0]
        p6[1]=p2[1]-width/2
        p6[2]=p2[2]
    a=(p2[1]-p1[1])*(p3[2]-p1[2])-(p3[1]-p1[1])*(p2[2]-p1[2])
    b=(p2[2]-p1[2])*(p3[0]-p1[0])-(p3[2]-p1[2])*(p2[0]-p1[0])
    c=(p2[0]-p1[0])*(p3[1]-p1[1])-(p3[0]-p1[0])*(p2[1]-p1[1])
    t=height/(2*(a**2+b**2+c**2)**0.5)
    p_3[0]=p3[0]+a*t
    p_3[1]=p3[1]+b*t
    p_3[2]=p3[2]+c*t
    p_2[0]=p3[0]-a*t
    p_2[1]=p3[1]-b*t
    p_2[2]=p3[2]-c*t
    p_4[0]=p4[0]+a*t
    p_4[1]=p4[1]+b*t
    p_4[2]=p4[2]+c*t
    p_1[0]=p4[0]-a*t
    p_1[1]=p4[1]-b*t
    p_1[2]=p4[2]-c*t
    p_7[0]=p5[0]+a*t
    p_7[1]=p5[1]+b*t
    p_7[2]=p5[2]+c*t
    p_6[0]=p5[0]-a*t
    p_6[1]=p5[1]-b*t
    p_6[2]=p5[2]-c*t
    p_8[0]=p6[0]+a*t
    p_8[1]=p6[1]+b*t
    p_8[2]=p6[2]+c*t
    p_5[0]=p6[0]-a*t
    p_5[1]=p6[1]-b*t
    p_5[2]=p6[2]-c*t
    list = [p_3,p_7,p_6,p_2,p_4,p_8,p_5,p_1]
    new = []
    for i in list:
        a = np.ndarray.tolist(i)
        new.append(a)
    # print(new)
    return new

def pengzhuang(mian,xian):
    n=mian[0]
    m=mian[1]
    o=mian[2]
    q1=xian[0]
    q2=xian[1]
    p=((n[1]-m[1])*(o[2]-m[2])-(o[1]-m[1])*(n[2]-m[2]),(n[2]-m[2])*(o[0]-m[0])-(o[2]-m[2])*(n[0]-m[0]),(n[0]-m[0])*(o[1]-m[1])-(o[0]-m[0])*(n[1]-m[1]))
    P=(p[0]**2+p[1]**2+p[2]**2)**0.5
    d1=(p[0]*(q1[0]-m[0])+p[1]*(q1[1]-m[1])+p[2]*(q1[2]-m[2]))
    d2=(p[0]*(q2[0]-m[0])+p[1]*(q2[1]-m[1])+p[2]*(q2[2]-m[2]))
    if d1==d2:
        return None
    else:
        u=d1/(d1-d2)
        t=[q1[0]-u*(q1[0]-q2[0]),q1[1]-u*(q1[1]-q2[1]),q1[2]-u*(q1[2]-q2[2])]
        j=-1
        h=0
        while j<2:
            j=j+1
            if t[j]>max(m[j],n[j],o[j])or t[j]<min(m[j],n[j],o[j]):
                h=1
            elif t[j]>max(q1[j],q2[j])or t[j]<min(q1[j],q2[j]):
                h=1
        if h==0:
            return t
        else:
            return None

#输入管道和障碍物，输出每段管道分别碰撞的障碍物
def pzjc(pipes,blocks):
    b = []
    b1 = []
    f = []
    g = []
    h = []
    k = []
    j = []
    o =[]
    block1 = []
    block2 = []
    block3 = []
    for w in range(len(pipes)):
        a = pipes[w]
        a1 = pipe_to_pipes(a)
        for v in range(len(blocks)):
            d1 = blocks[v]
            d2 = dian2(d1[0],d1[1],d1[2],d1[3])
            d = point_8_to_3(d2)
            e = point_3_to_surface_6(d)
            for p in range(len(a1)):
                i = 0
                while i < 6:
                    f = pengzhuang(e[i],a1[p])
                    # print(f)
                    if f != None:
                        g.append(blocks[v])
                        i += 1
                    else:
                        i+=1
            if len(g) != 0:
                k.append(g[0])
            else:
                k.append(o)

            g.clear()
            block1 = [k[i:i+len(blocks)] for i in range(0,len(k),len(blocks))]
    # print(block1)
    for sam in block1:
        if sam:
            block2 = []
            for x in sam:
                if x:
                    block2.append(x)
            block3.append(block2)

    # print("每个管道分别碰撞的障碍物为",block3)
    return block3
